package pathfind;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.PriorityQueue;
import pathfind.mainTestClass.Point;

/**
 * A* algorithm implementation using the method design pattern.
 *
 * @author Giuseppe Scrivano
 */
public abstract class AStarAbstract<T> {

    protected Object additionalParam;
    private boolean fullPathCalculated;
    private final PriorityQueue<AStarPath> paths;
    private final Map<T, Double> mindists;
    private double lastCost;
    private int expandedCounter;
    private double closestLength = Double.MAX_VALUE;
    private T lastClosestPoint = null;
    private final List<T> successorsList = new ArrayList<>(20);

    public abstract AStarAbstract<T> newInstance(T end, Object additionalParam);

    /**
     * Check if the current node is a goal for the problem.
     *
     * @param node The node to check.
     * @return <code>true</code> if it is a goal, <code>false</else> otherwise.
     */
    protected abstract boolean isGoal(T node);

    /**
     * Generate the successors for a given node.
     *
     * @param node The node we want to expand.
     * @return A list of possible next steps.
     */
    protected abstract List<T> generateSuccessors(T node, List<T> successorsOutputList);

    /**
     * Check how many times a node was expanded.
     * @return A counter of how many times a node was expanded.
     */
    public int getExpandedCounter() {
        return expandedCounter;
    }

    /**
     * Default c'tor.
     */
    public AStarAbstract() {
        paths = new PriorityQueue<>();
        mindists = new HashMap<>(1000);
        expandedCounter = 0;
        lastCost = 0.0;
    }

    /**
     * Cost for the operation to go to
     * <code>to</code> from
     * <code>from</from>.
     *
     * @param from The node we are leaving.
     * @param to The node we are reaching.
     * @return The cost of the operation.
     */
    protected abstract double g(T from, T to);

    /**
     * Estimated cost to reach a goal node. An admissible heuristic never gives
     * a cost bigger than the real one.
     * <code>from</from>.
     *
     * @param from The node we are leaving.
     * @param to The node we are reaching.
     * @return The estimated cost to reach an object.
     */
    protected abstract double h(T from, T to);

    protected abstract double squaredLengthToGoal(T cell);

    /**
     * Total cost function to reach the node
     * <code>to</code> from
     * <code>from</code>.
     *
     * The total cost is defined as: f(x) = g(x) + h(x).
     *
     * @param from The node we are leaving.
     * @param to The node we are reaching.
     * @return The total cost.
     */
    protected double f(AStarPath p, T from, T to) {
        double g = g(from, to) + ((p.parent != null) ? p.parent.g : 0.0);
        double h = h(from, to);
        p.g = g;
        p.f = g + h;
        return p.f;
    }

    public T getLastClosestPoint() {
        return lastClosestPoint;
    }

    /**
     * Expand a path.
     *
     * @param path The path to expand.
     */
    private void expand(AStarPath<T> path) {
        T p = path.getPoint();
        double lengthToGoal = squaredLengthToGoal(p);
        if (lengthToGoal < closestLength) {
            closestLength = lengthToGoal;
            lastClosestPoint = p;
        }
        Double min = mindists.get(path.getPoint());
        /*
         * If a better path passing for this point already exists then
         * don't expand it.
         */
        if (min == null || min > path.f) {
            mindists.put(path.getPoint(), path.f);
        } else {
            return;
        }
        successorsList.clear();
        generateSuccessors(p, successorsList);
        for (T t : successorsList) {
            AStarPath newPath = new AStarPath(path);
            newPath.setPoint(t);
            f(newPath, path.getPoint(), t);
            paths.offer(newPath);
            //printqueue();
        }

        expandedCounter++;
    }

    /**
     * Get the cost to reach the last node in the path.
     *
     * @return The cost for the found path.
     */
    public double getCost() {
        return lastCost;
    }

    /**
     * Find the shortest path to a goal starting from
     * <code>start</code>.
     *
     * @param start The initial node.
     * @return A list of nodes from the initial point to a goal,
     * <code>null</code> if a path doesn't exist.
     */
    public List<T> computeWithFallback(T start) {
        List<T> path = compute(start);
        if (path == null) {
            fullPathCalculated = false;
            AStarAbstract newPathfinder = newInstance(getLastClosestPoint(), additionalParam);
            return newPathfinder.compute(start);
        } else {
            fullPathCalculated = true;
        }
        return path;
    }

    public boolean isFullPathCalculated() {
        return fullPathCalculated;
    }
    static int num = 0;

    public List<T> compute(T start) {
        if (isGoal(start)) {
            List<T> list = new ArrayList<>();
            list.add(start);
            return list;
        }
        AStarPath root = new AStarPath();
        root.setPoint(start);
        /* Needed if the initial point has a cost.  */
        f(root, start, start);
        expand(root);
        for (;;) {
            AStarPath<T> p = paths.poll();
            /*System.out.print("poll-");
            printAstar(p);
            System.out.println("\n\n\n");*/
            if (p == null) {
                lastCost = Double.MAX_VALUE;
                return null;
            }

            T last = p.getPoint();
            lastCost = p.g;
            if (isGoal(last)) {
                List<T> retPath = new ArrayList<>(1000);
                for (AStarPath i = p; i != null; i = i.parent) {
                    retPath.add(0, (T) i.getPoint());
                }

                return retPath;
            }

            expand(p);
        }
    }

   /* private void printAstar(AStarPath p) {
        if (p != null) {
            System.out.println(String.format("%d: %d %d f=%.2f g=%.2f", num, ((Point) p.getPoint()).getX(), ((Point) p.getPoint()).getY(), p.f, p.g));
        }
    }

    private void printqueue() {
        for (AStarPath p : paths) {
            printAstar(p);
        }
        num++;
    }*/
}
